clear
ultimateLon =-79.948681;
ultimateLat =40.436606;
boat_x = [0 1];                       % define the end points of the stick figure
boat_y = [0 0];
desiredH_x = [1 20];
desiredH_y = [0 0];
pause(5);
interval = 0.00003;
interLat = 0;
interLon = 0;
STEP = 2;
isfirst = 1;
%fp = fopen('~/autoBoat/log/log-1372702460.txt');
%1373306119
%1373306881
%1373307739 overshoot
fp = fopen('~/autoBoat/log/88/log-1373307739.txt');
c = fscanf(fp,'%s');
fclose(fp);
indexLat = strfind(c, '<lat>')+length('<lat>');
indexLatE = strfind(c, '</lat>') -1;
indexLon = strfind(c, '<lon>')+length('<lon>');
indexLonE = strfind(c, '</lon>') -1;
indexCompass = strfind(c, '<compass>')+length('<compass>');
indexCompassE = strfind(c, '</compass>') -1;
indexDesiredHeading = strfind(c, '<desiredHeading>')+length('<desiredHeading>');
% indexGoalLat = strfind(c, '<latitude>')+length('<latitude>');
% indexGoalLatE = strfind(c, '</latitude>') -1;
% indexGoalLon = strfind(c, '<longitude>')+length('<longitude>');
% indexGoalLonE = strfind(c, '</longitude>') -1;
% indexObsLat = strfind(c, '<obsLat>')+length('<obsLon>');
% indexObsLatE = strfind(c, '</obsLat>') -1;
% indexObsLon = strfind(c, '<obsLon>')+length('<obsLon>');
% indexObsLonE = strfind(c, '</obsLon>') -1;

% lat =  str2double(myParse(c, 'lat'));
% lon =  str2double(myParse(c, 'lon'));
% goalLat =  str2double(myParse(c, 'goalLat'));
% goalLon =  str2double(myParse(c, 'goalLon'));
% 
% Xmin = min(lon, ultimateLon)-0.009;
% Xmax = max(lon, ultimateLon)+0.009;
% Ymin = min(lat, ultimateLat)-0.009;
% Ymax = max(lat, ultimateLat)+0.009;
% 
% axis([Xmin Xmax Ymin Ymax]);
% hold on;
% M = ceil((Ymax - Ymin)/interval);
% N = ceil((Xmax - Xmin)/interval);


obs(10) = struct( ...
    'startIndex',[], ...
    'startValue',[], ...  test
    'endIndex',[], ...
    'endValue',[]);

%mapping = @(x,y) (floor(N*(x - Xmin)/(Xmax - Xmin)) -1 )* M + floor(M*(y - Ymin)/(Ymax - Ymin));

lon = 0;
lat = 0;
goalLon = 0;
goalLat = 0;

startHDL = plot(lon,lat, 'x');
startTxt = text(lon,lat, sprintf('Start'), ...
    'Color',[0.2 0.2 0.2], 'FontSize',8, ...
    'HorizontalAlignment','left', 'VerticalAlignment','top');
hold on
endHDL = plot(goalLon,goalLat, 'd');
endTxt = text(goalLon,goalLat, sprintf('end'), ...
    'Color',[0.2 0.2 0.2], 'FontSize',8, ...
    'HorizontalAlignment','left', 'VerticalAlignment','top');
hold on
pathHDL = line('XData',0, 'YData',0, 'Color','r', ...
    'Marker','d', 'MarkerSize',1, 'LineWidth',2);
obsHDL = line('XData',0, 'YData',0, 'Color','b', ...
            'Marker','o', 'MarkerSize',6, 'LineWidth',2);
boatHDL=plot(boat_x,boat_y,'EraseMode','background'); % connect the end points,
                                               % and use "EraseMode" to REDUCE FLASHING
hold on
                                               
desiredHHDL = plot(desiredH_x, desiredH_y, '--','EraseMode','background'); 
hold on
headHDL = line('XData',boat_x(1), 'YData',boat_y(1), 'Color','b', ...
    'Marker','o', 'MarkerSize',6, 'LineWidth',2);
headTxt = text(boat_x(1), boat_y(1), sprintf('(%.3f,%.3f)',boat_x(1),boat_y(1)), ...
    'Color',[0.2 0.2 0.2], 'FontSize',8, ...
    'HorizontalAlignment','left', 'VerticalAlignment','top');

goalHDL = line('XData',goalLon, 'YData',goalLat, 'Color','r', ...
    'Marker','d', 'MarkerSize',6, 'LineWidth',2);
goalTxt = text(0, 0, sprintf('(%.3f,%.3f)',0,0), ...
    'Color',[0.2 0.2 0.2], 'FontSize',8, ...
    'HorizontalAlignment','left', 'VerticalAlignment','top');

interHDL = line('XData',0, 'YData',0, 'Color','r', ...
    'Marker','d', 'MarkerSize',6, 'LineWidth',2);
interTxt = text(0, 0, sprintf('(%.3f,%.3f)',0,0), ...
    'Color',[0.2 0.2 0.2], 'FontSize',8, ...
    'HorizontalAlignment','left', 'VerticalAlignment','top');

obsDHDL = line('XData',0, 'YData',0, 'Color','k', ...
    'Marker','p', 'MarkerSize',6, 'LineWidth',2);
obsTxt = text(0, 0, sprintf('(%.3f,%.3f)',0,0), ...
    'Color',[0.2 0.2 0.2], 'FontSize',8, ...
    'HorizontalAlignment','left', 'VerticalAlignment','top');

set(gca,'DataAspectRatio',[1 1 1]);            % make x,y-axis relative scaling equal
xlabel('Longitude');
ylabel('Latitude');
R = 6371000;
for i = 1 : length(indexLon)
%     if (2*(i-diffCounter)-1) < length(indexGoalLon)
%         if indexLon(i) < indexGoalLon((i-diffCounter))
%             diffCounter = diffCounter+1;
%             continue;
%         end
%     else
%         break
%     end
    
    lon = str2double(c(indexLon(i):indexLonE(i)));
    lat = str2double(c(indexLat(i):indexLatE(i)));
    if isnan(lat)
        continue;
    end
    compass = str2double(c(indexCompass(i):indexCompassE(i)));
    compass = compass/180*pi;
    
%     goalLon = str2double(c(indexGoalLon(i-diffCounter):indexGoalLonE(i-diffCounter)));
%     goalLat = str2double(c(indexGoalLat(i-diffCounter):indexGoalLatE(i-diffCounter)));

%     obsLon = str2double(c(indexObsLon(i):indexObsLonE(i)));
%     obsLat = str2double(c(indexObsLat(i):indexObsLatE(i)));
%     obsHeading = atan((obsLon - compass*180/pi)/(obsLat - compass*180/pi));
%     obsDistance = (obsLat - compass*180/pi) / cos(obsHeading);
%     obsLat =asin(sin(lat*pi/180)*cos(obsDistance/R) + cos(lat*pi/180)*sin(obsDistance/R)*cos(obsHeading));
%     obsLon =lon + atan2(sin(obsHeading)*sin(obsDistance/R)*cos(lat*pi/180), cos(obsDistance/R)-sin(lat*pi/180)*sin(obsLat))*180/pi+ 0.0001;
%     obsLat = obsLat *180 /pi - 0.00001;
    
    Xmin = min(lon, ultimateLon)-0.0003;
    Xmax = max(lon, ultimateLon)+0.0003;
    Ymin = min(lat, ultimateLat)-0.0003;
    Ymax = max(lat, ultimateLat)+0.0003;

    axis([Xmin Xmax Ymin Ymax]);
    hold on;
    M = ceil((Ymax - Ymin)/interval);
    N = ceil((Xmax - Xmin)/interval);
    map = zeros(M,N);
    mapping=@(x, y) (floor(N*(x - Xmin)/(Xmax - Xmin)) -1 )* M + floor(M*(y - Ymin)/(Ymax - Ymin));
    if ~isnan(lon)
        start = mapping(lon, lat);
        map(start) = 1;
    else
        continue
    end
    endpoint = mapping(ultimateLon, ultimateLat);
    map(endpoint) = 10;
%     if ~isnan(obsLon) && obsLon ~=0
%         map(mapping(obsLon, obsLat)) = 9;
%     end
% 
%     obsLat =  str2double(myParse(c, 'obsLat'));
%     obsLon =  str2double(myParse(c, 'obsLon'));
    
    set(goalHDL, 'XData',ultimateLon, 'YData',ultimateLat);
    set(goalTxt, 'Position',[ultimateLon ultimateLat], ...
        'String',sprintf('Goal(%.5f,%.5f)',[ultimateLon ultimateLat]));   
%     set(obsDHDL, 'XData',obsLon, 'YData',obsLat);
%     set(obsTxt, 'Position',[obsLon obsLat], ...
%         'String',sprintf('Obstacle(%.5f,%.5f)',[obsLon obsLat]));
%    obsCounter = str2double(myParse(c, 'obsCounter'));
%     obsCounter = 0;
%     for k = 1: obsCounter
%         obs(k).startLat = str2double(myParse(c, strcat('sLat',num2str(k-1))));
%         obs(k).startLon = str2double(myParse(c, strcat('sLon',num2str(k-1))));
%         obs(k).endLat = str2double(myParse(c, strcat('eLat',num2str(k-1))));
%         obs(k).endLon = str2double(myParse(c, strcat('eLon',num2str(k-1))));
%         set(obsHDL, 'XData',[obs(k).startLon obs(k).endLon], 'YData',[obs(k).startLat obs(k).endLat]);
%         if (obs(k).startLon > obs(k).endLon)
%             startLat = obs(k).endLat;
%             startLon = obs(k).endLon;
%             diffLon = obs(k).startLon - startLon;
%             diffLat = obs(k).startLat - startLat;
%         else
%             startLat = obs(k).startLat;
%             startLon = obs(k).startLon;
%             diffLon = obs(k).endLon - startLon;
%             diffLat = obs(k).endLat - startLat;
%         end
% 
%         abs_diffLat = abs(diffLat);
%         
%         if abs_diffLat > diffLon
%             for m = 0: round(abs_diffLat/interval)
%                 map(mapping(startLon+m*diffLon/abs_diffLat*interval, startLat+m*interval*sign(diffLat))) = 9;
%             end
%         else
%             for m = 0: round(diffLon/interval)
%                 map(mapping(startLon+m*interval, startLat+m*abs_diffLat/diffLon*interval*sign(diffLat))) = 9;
%             end
%         end
%     end
%     
%     if obsCounter > 0
%         fillmap(map,M, N);
%     end
%     
   if lat > ultimateLat + interval*5
        for o = ultimateLon-interval*5:interval: ultimateLon+interval*5
    %         midpointY = round((mod(startF,M)+mod(endpoint,M))/2);
    %         midpointX = round((startF+endpoint)/M/2);
    %         for o = midpointX - 5:midpointX + 5
    %             map(midpointY,o) = 9;
    %         
    %         end
            map(mapping(o, ultimateLat + interval*5)) = 9;
        end
    end
        
    [obsY, obsX] = find(map == 9);
    obsY = Ymin + obsY*interval;
    obsX = Xmin + obsX*interval;
    set(obsHDL, 'XData', obsX, 'YData', obsY);
    
    if (isfirst)
        [path_x,path_y] = Astar(map,M,N,start,endpoint,Ymax,Ymin,Xmin);

    end  
    if ((abs(interLat - lat) < interval) && (abs(interLon - lon) < interval))
        [path_x,path_y] = Astar(map,M,N,start,endpoint,Ymax,Ymin,Xmin);
    end
    
    if (isfirst)
        if length(path_x) > STEP || length(path_y) > STEP
            interLat = path_y(STEP);
            interLon = path_x(STEP);
        else
            interLat = ultimateLat;
            interLon = ultimateLon;
        end
    isfirst = 0;
    end   
    
    set(startHDL, 'XData',path_x(1), 'YData',path_y(1));

    set(startTxt, 'Position',[path_x(1) path_y(1)], ...
            'String',sprintf('AStar'));


    set(endHDL, 'XData',path_x(end), 'YData',path_y(end));

    set(endTxt, 'Position',[path_x(end) path_y(end)], ...
            'String',sprintf('End'));

    set(pathHDL,'XData',path_x,'YData',path_y);



    desiredHeading = str2double(c(indexDesiredHeading(i):indexDesiredHeading(i)+7));
 
    desiredH = desiredHeading/180*pi;
    boat_y=[lat-0.0001*cos(compass)  lat];
    boat_x=[lon-0.0001*sin(compass)  lon];
    desiredH_y = [lat lat+0.0001*cos(desiredH)];
    desiredH_x = [lon lon+0.0001*sin(desiredH)];

    set(boatHDL,'XData',boat_x,'YData',boat_y);       % REDUCE FLASHING
    set(headHDL, 'XData',lon, 'YData',lat);
    set(headTxt, 'Position',[lon lat], ...
        'String',sprintf('Boat(%.5f,%.5f)',[lon lat]));
    set(desiredHHDL,'XData',desiredH_x,'YData',desiredH_y);
    set(interHDL, 'XData',interLat, 'YData',interLat);
    set(interTxt, 'Position',[interLon interLat], ...
        'String',sprintf('Intermediate(%.5f,%.5f)',[interLon interLat]));
    
    if ((abs(interLat - lat) < interval) && (abs(interLon - lon) < interval))

    interLat = path_y(STEP);
    interLon = path_x(STEP);
 
%     set(interHDL, 'XData',interLat, 'YData',interLat);
%     set(interTxt, 'Position',[interLon interLat], ...
%         'String',sprintf('Intermediate(%.5f,%.5f)',[interLon interLat]));
%     

    end
    
    
    drawnow         % forces Matlab to redraw the plot at the current values
    pause(.2)



end